/**
 * 
 */
package due.pc.pcshoot.sensor;

import java.util.Stack;


import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.util.Log;

/**
 * @author manhhung.vu
 *
 */
public class MotionDetector2 implements SensorEventListener
{
	//===============================================================/
	//Constants
	//===============================================================/
	
	
	//===============================================================/
	//Fields
	//===============================================================/
	public boolean mAccelerometerLogEnable = false;
	public boolean mOrientationLogEnable = false;
	
	private float[] mGravity = new float[3];
	private float[] mLastAccelerometerData = new float[3];
	
	private float[] mLastOrientationDelta = new float[3];
	private float[] mLastOrientationData = new float[3];
	private float[] mStartOrientationData = new float[3];
	private boolean mOrientationDataMessured = false;
	
	private FloatArrayPool mAccerlerometerDataPool = new FloatArrayPool(3);
	private FloatArrayPool mOrientationDeltaPool = new FloatArrayPool(3);
	private FloatArrayPool mOrientationDataPool = new FloatArrayPool(3);
	
	private float[] mAccelerometerData;
	private float[] mOrientationDeltaData;
	private float[] mOrientationData;
	
	
	private boolean mWithPicoProjector = false;
	
	//===============================================================/
	//Constructors
	//===============================================================/
	public MotionDetector2(boolean pWithPicoProjector) {
		mWithPicoProjector = pWithPicoProjector;
	}
	
	//===============================================================/
	//Inherited Methods
	//===============================================================/
	@Override
	public void onAccuracyChanged(Sensor arg0, int arg1) {
		// TODO Auto-generated method stub
		
	}

	@Override
	public void onSensorChanged(SensorEvent pEvent) 
	{
		float[] data = pEvent.values;
		
		switch (pEvent.sensor.getType()) {
		case Sensor.TYPE_ACCELEROMETER:	
			
			float[] lastSavedData = new float[3]; 
			
			//elminate Ground Gravity
			final float alpha = 0.9f;
			
	        mGravity[0] = alpha * mGravity[0] + (1 - alpha) * data[0];
	        mGravity[1] = alpha * mGravity[1] + (1 - alpha) * data[1];
	        mGravity[2] = alpha * mGravity[2] + (1 - alpha) * data[2];

	        data[0] = data[0] - mGravity[0];
	        data[1] = data[1] - mGravity[1];
	        data[2] = data[2] - mGravity[2];
	        
	        //eliminate noise
	        final float accelerometerThresholdStep = 0.1f;
	        System.arraycopy(mLastAccelerometerData, 0, lastSavedData, 0, 3);

	        for (int i = 0; i < 3; i++)
	        {
	        	float delta = data[i] - mLastAccelerometerData[i];
	        	float absDelta = Math.abs(delta);
	        	
	        	if (absDelta >= accelerometerThresholdStep) {
	        		lastSavedData[i] = data[i];
	        	}
	        }
	        
	        mLastAccelerometerData = lastSavedData;
	        mAccerlerometerDataPool.pushItem(lastSavedData);
	        
	        String aText = "AccelerometerData ax " + data[0]  + ", ay " + data[1] + ", az " + data[2];
			if (mAccelerometerLogEnable) Log.d("Accelerometer", aText);
			
			aText = "Noise eliminated AccelerometerData ax " + mLastAccelerometerData[0]  + ", ay " + mLastAccelerometerData[1] + ", az " + mLastAccelerometerData[2];
			if (mAccelerometerLogEnable) Log.d("Accelerometer", aText);
			
			break;

		case Sensor.TYPE_ORIENTATION:
			
			float[] lastSavedDelta = new float[3];
			
			//eliminate noise
			final float orientationThresholdStep = 1.0f;
			
			if (mOrientationDataMessured == false) 
			{
				System.arraycopy(data, 0, mLastOrientationData, 0, 3);
				System.arraycopy(data, 0, mStartOrientationData, 0, 3);
				mOrientationDataMessured = true;
			} 
			
			if (mOrientationLogEnable) Log.d("Orientation", "old a "+mLastOrientationData[0]+" p "+mLastOrientationData[1]+" r " +mLastOrientationData[2]);
			if (mOrientationLogEnable) Log.d("Orientation", "new a "+data[0]+" p "+data[1]+" r " +data[2]);
			
			for (int i = 0; i < 3; i++) 
			{
				float delta = data[i] - mLastOrientationData[i];
	        	float absDelta = Math.abs(delta);
	        	
	        	if (absDelta >= orientationThresholdStep) {
	        		lastSavedDelta[i] = delta;
	        	}
			}
			
			System.arraycopy(data, 0, mLastOrientationData, 0, 3);
			mLastOrientationDelta = lastSavedDelta;
			mOrientationDeltaPool.pushItem(mLastOrientationDelta);
			
			mOrientationDataPool.pushItem(mLastOrientationData);
			
			String oText = "Noise eliminated Orientation Delta  Azimuth " + mLastOrientationDelta[0]  + ", Pitch " + mLastOrientationDelta[1] + ", Roll " + mLastOrientationDelta[2];
			if (mOrientationLogEnable) Log.d("Orientation", oText);
			
			break;
		};
	}
	
	
	//===============================================================/
	//Methods
	//===============================================================/
	public void prepareSensorData() 
	{
		mAccelerometerData = getAccelerometerData();
		mOrientationDeltaData = getOrientationDeltaData();
		mOrientationData = getOrientationData();
	}
	
	//===============================================================/
	//Getters & Setters
	//===============================================================/
	
	public float[] getAccelerometerData() {
		return mAccelerometerData = mAccerlerometerDataPool.obtainMedianOfItems();
	}
	
	public float[] getOrientationDeltaData() {
		return mOrientationDeltaData = mOrientationDeltaPool.obtainMedianOfItems();
	}
	
	public float[] getOrientationData() {
		return mOrientationData = mOrientationDataPool.obtainMedianOfItems();
	}
	
	public float getLeftRightAcceleration() {
		if (mWithPicoProjector) {
			return (mAccelerometerData != null) ? -mAccelerometerData[0] : 0;
		}
		return (mAccelerometerData != null) ? mAccelerometerData[1] : 0;
	}
	
	public float getTopBottomAcceleration() {
		if (mWithPicoProjector) {
			return (mAccelerometerData != null) ? -mAccelerometerData[1] : 0;
		}
		return (mAccelerometerData != null) ? mAccelerometerData[0] : 0; 
	}
	
	public float getLeftRightDeltaAngel() {
		if (mWithPicoProjector) {
			return (mOrientationDeltaData != null) ? -mOrientationDeltaData[2] : 0;
		}
		return (mOrientationDeltaData != null) ? -mOrientationDeltaData[1] : 0;
	}
	
	public float getTopBottomDeltaAngel() {
		if (mWithPicoProjector) {
			return (mOrientationDeltaData != null) ? mOrientationDeltaData[1] : 0;
		}
		return (mOrientationDeltaData != null) ? mOrientationDeltaData[2] : 0;
	}
	
	public float getLeftRightAngel() {
		if (mWithPicoProjector) {
			return (mOrientationData != null) ? -(mOrientationData[2] - mStartOrientationData[2]) : 0;
		}
		return (mOrientationData != null) ? -(mOrientationData[1] - mStartOrientationData[1]) : 0;
	}
	
	public float getTopBottomAngel() {
		if (mWithPicoProjector) {
			return (mOrientationData != null) ? mOrientationData[1] - mStartOrientationData[1] : 0;
		}
		return (mOrientationData != null) ? mOrientationData[2] - mStartOrientationData[2] : 0;
	}
	
	//===============================================================/
	//Static Methods
	//===============================================================/
	
	
	//===============================================================/
	//Inner Classes
	//===============================================================/
	class FloatArrayPool 
	{
		//===============================================================/
		//Fields
		//===============================================================/
		private Stack<Float[]> mPool;
		private int mItemSize;
		private int mItemCount;
		
		//===============================================================/
		//Constructors
		//===============================================================/
		public FloatArrayPool(int pItemSize) 
		{
			mPool = new Stack<Float[]>();
			mItemSize = pItemSize;
			mItemCount = 0;
		}
		
		//===============================================================/
		//Methods
		//===============================================================/
		synchronized public float[] obtainMedianOfItems()
		{
			float[] result = new float[mItemSize];
			int itemCount = mItemCount;
			
			while (mItemCount > 0)
			{
				Float[] item = mPool.pop();
				for (int i = 0; i < mItemSize; i++) {
					result[i] += item[i];
				}
				mItemCount--;
			}
						
			if (itemCount != 0) {
				for (int i = 0; i < mItemSize; i++) result[i] /= itemCount;
			}
			
			return result;
		}
		
		synchronized public void pushItem(float[] pItem)
		{
			if (pItem != null) 
			{
				Float[] item = new Float[mItemSize];
				for (int i = 0; i < mItemSize; i++) item[i] = pItem[i];
				
				mPool.push(item);
				mItemCount++;
			}
		}
		
		//===============================================================/
		//Getters & Setters
		//===============================================================/
	}
}
